#include "imu.h"
#include "math.h"
#include "mpu6050_new.h"

// 四元数结构体
quaternion_t quaternion = {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};

static float dt = 0.010f;       // 10ms时间间隔

// 四元数初始化
void quaternion_init(void)
{
    quaternion.q0 = 1.0f;
    quaternion.q1 = 0.0f;
    quaternion.q2 = 0.0f;
    quaternion.q3 = 0.0f;
    quaternion.roll = 0.0f;
    quaternion.pitch = 0.0f;
    quaternion.yaw = 0.0f;
}

// 四元数更新（10ms周期调用）
void quaternion_update(void)
{
    float gx, gy, gz;           // 角速度（弧度/秒）
    // float ax, ay, az;           // 加速度（单位/g）

    // 获取陀螺仪数据
    MPU6050_GetGyro();
    // MPU6050_GetAcc();

    gx = ((mpu6050_gyro_x / 10 * 10 / 14.3f) * (M_PI / 180.0f));
    gy = ((mpu6050_gyro_y / 10 * 10 / 14.3f) * (M_PI / 180.0f));
    gz = ((mpu6050_gyro_z / 10 * 10 / 14.3f) * (M_PI / 180.0f));

    // 四元数微分方程计算
    float q0 = quaternion.q0;
    float q1 = quaternion.q1;
    float q2 = quaternion.q2;
    float q3 = quaternion.q3;

    quaternion.q0 += 0.5f * dt * (-q1*gx - q2*gy - q3*gz);
    quaternion.q1 += 0.5f * dt * ( q0*gx + q2*gz - q3*gy);
    quaternion.q2 += 0.5f * dt * ( q0*gy - q1*gz + q3*gx);
    quaternion.q3 += 0.5f * dt * ( q0*gz + q1*gy - q2*gx);

    // 四元数归一化
    float norm = sqrtf(quaternion.q0*quaternion.q0 +
                       quaternion.q1*quaternion.q1 +
                       quaternion.q2*quaternion.q2 +
                       quaternion.q3*quaternion.q3);

    quaternion.q0 /= norm;
    quaternion.q1 /= norm;
    quaternion.q2 /= norm;
    quaternion.q3 /= norm;

    // 转换为欧拉角（Z-Y-X顺序）
    quaternion.roll  = atan2f(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2)) * (180.0f/M_PI);
    quaternion.pitch = asinf(2*(q0*q2 - q3*q1)) * (180.0f/M_PI);
    quaternion.yaw   = atan2f(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3)) * (180.0f/M_PI); // 角度范围在-180~180
}

float MPU6050_GetYaw(void)
{
    return quaternion.yaw;
}

float MPU6050_GetRoll(void)
{
    return quaternion.roll;
}

float MPU6050_GetPitch(void)
{
    return quaternion.pitch;
}